#include "WPILib.h"
#include "PandaConv.h"
#include "PandaDrive.h"
#include "PandaDump.h"
#include "Compressor.h"
#include "DriverStationLCD.h"
#include "Encoder.h"
#include <cmath>

const int tankButton = 4;
const int arcadeButton = 5;
const int nonslipButton = 2;
const int slipButton = 3;
const int dumpButton = 6;
const int increaseAccelButton = 8;
const int decreaseAccelButton = 9;
const int leftConvOffButton = 11;
const int rightConvOffButton = 10;
const int Trigger = 1;
const int leftKillButton = 7;
const int rightKillButton = 8;
const int rightKillButton2 = 10;
const float accel_threshhold = 0.15;
const int accelPrecision = 2;

// Round a float to an int.
int round(float n)
{
	if (n > 0) return int (n + .5);
	else return int (n - .5);
}

// Round a float to d decimal digits of precision.
float round(float n, unsigned int d)
{
	float tens = pow(10, d);
	return round(n * tens) / tens;
}

class PandaBot: public SimpleRobot
{
	PandaDrive *pandaDrive; // robot drive system
	Joystick *leftStick, *rightStick;
	DriverStation *ds; // driver station
	DriverStationLCD *dslcd;
	PandaConv* pandaConv;
	PandaDump* pandaDump;
	Accelerometer* xAccel;
	Accelerometer* yAccel;
	float m_xAccelBase, m_yAccelBase;
	Compressor* pandaComp;
	Encoder* rightEncoder;
	Encoder* leftEncoder;
	bool m_kill;
	//void SelfTest();
	public:
		PandaBot(void)
		{
			ds = DriverStation::GetInstance();
			dslcd = DriverStationLCD::GetInstance();
			// Set up the joysticks.
			leftStick = new Joystick(2);
			rightStick = new Joystick(1);
			// Set up the components.
			pandaDrive = new PandaDrive(1, 2, leftStick, rightStick, tankButton, arcadeButton, nonslipButton, slipButton, increaseAccelButton, decreaseAccelButton);
			pandaConv = new PandaConv(3, 4, 4, 1, leftStick, leftConvOffButton, rightConvOffButton);
			pandaDump = new PandaDump(1, 8, leftStick, dumpButton, 3);
			pandaComp = new Compressor(4,8);
			// Set up the encoders.
			rightEncoder = new Encoder(8, 9, true);
			rightEncoder->Reset();
			rightEncoder->Start();
			leftEncoder = new Encoder(10, 11, false);
			leftEncoder->Reset();
			leftEncoder->Start();
			// Set up the accelerometer.
			xAccel = new Accelerometer(2, 6);
			yAccel = new Accelerometer(2, 7);
			xAccel->SetZero(0);
			yAccel->SetZero(0);
			Wait(0.1);
			xAccel->SetZero(xAccel->GetAcceleration());
			yAccel->SetZero(yAccel->GetAcceleration());
			xAccel->SetSensitivity(.3);
			yAccel->SetSensitivity(.3);
			m_xAccelBase = xAccel->GetAcceleration();
			m_yAccelBase = yAccel->GetAcceleration();
			// Set up the watchdog.
			GetWatchdog().SetExpiration(100);
			m_kill = false;
		}
		// Get accelerometer values.
		float XAccel()
		{
			double currentTime = GetTime();
			int i = 0;
			static float last;
			float addedAccel = 0;
			while (GetTime() - currentTime < .1)
			{
				i++;
				addedAccel += xAccel->GetAcceleration();
			}
			addedAccel /= i;
			float accel = round(addedAccel - m_xAccelBase, accelPrecision);
			float deltaAccel = accel - last;
			last = accel;
			return deltaAccel;
		}
		float YAccel()
		{
			double currentTime = GetTime();
			int i = 0;
			static float last;
			float addedAccel = 0;
			while (GetTime() - currentTime < .1)
			{
				i++;
				addedAccel += yAccel->GetAcceleration();
			}
			addedAccel /= i;
			float accel = round(addedAccel - m_yAccelBase, accelPrecision);
			float deltaAccel = accel - last;
			last = accel;
			return deltaAccel;
		}
		// Manage the robot with autonomous and teleop.
		void PandaBot::StartCompetition()
		{
			printf("This is *our* manager function.\n");
			while (1)
			{
				if (m_kill)
					return;
				while (IsDisabled())
					Wait(.01);
				if (IsAutonomous() && !m_kill)
				{
					Autonomous();
					while (IsAutonomous() && !IsDisabled())
						Wait(.01);
				}
				else if (!m_kill)
				{
					OperatorControl();
					while (IsOperatorControl() && !IsDisabled())
						Wait(.01);
				}
			}
		}
		// Do autonomous.
		void Autonomous(void)
		{
			dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Let's dance!         ");
			dslcd->UpdateLCD();
			// Turn off the watchdog.
			GetWatchdog().SetEnabled(false);
			printf("init accel = (%+.3f,%+.3f)\n", m_xAccelBase, m_yAccelBase);
			// Drive forward for two seconds, to get away from opposing thrower.
			pandaDrive->SetLeftRightMotorSpeeds(1, 1);
			float lastLeft = 1;
			float lastRight = 1;
			float leftMotorSpeed = 0;
			float rightMotorSpeed = 0;
			Wait(2);
			while (IsAutonomous()) {
				// Keep going forward.
				pandaDrive->SetLeftRightMotorSpeeds(1, 1);
				lastLeft = 1;
				lastRight = 1;
				// Get change in acceleration.
				float x_diff = XAccel();
				float y_diff = YAccel();
				//if (fabs(x_diff) > accel_threshhold || fabs(y_diff) > accel_threshhold)
					//printf("accel = (%+.3f,%+.3f)\n", x_diff, y_diff);
				float left = 0, right = 0, left2 = 0, right2 = 0;
				if (x_diff > accel_threshhold && y_diff > accel_threshhold)
				{
					printf("Hit from left back\n");
					left = 1;
					right = -.5;
				}
				else if (x_diff > accel_threshhold && y_diff < -accel_threshhold)
				{
					printf("Hit from left front\n");
					left = -1;
					right = -1;
					left2 = 1;
					right2 = -.5;
				}
				else if (x_diff < accel_threshhold && x_diff > -accel_threshhold && y_diff > accel_threshhold)
				{
					printf("Hit from back\n");
					continue;
				}
				else if (x_diff < -accel_threshhold && y_diff > accel_threshhold)
				{
					printf("Hit from right back\n");
					left = -.5;
					right = 1;
				}
				else if (x_diff < -accel_threshhold && y_diff < accel_threshhold && y_diff > -accel_threshhold)
				{
					printf("Hit from right\n");
					left = -.5;
					right = 1;
				}
				else if (x_diff < -accel_threshhold && y_diff < -accel_threshhold)
				{
					printf("Hit from right front\n");
					left = -1;
					right = -1;
					left2 = -.5;
					right2 = 1;
				}
				else if (x_diff < accel_threshhold && x_diff > -accel_threshhold && y_diff < -accel_threshhold)
				{
					printf("Hit from front\n");
					left = -1;
					right = -1;
					left2 = 1;
					right2 = -.5;
				}
				else if (x_diff > accel_threshhold && y_diff > -accel_threshhold && y_diff < accel_threshhold)
				{
					printf("Hit from left\n");
					left = 1;
					right = -.5;
				}
				else {
					continue;
				}
				// Turn as instructed (this may be a two-stage operation).
				for (int m = 0; m < 15; m++)
				{
					leftMotorSpeed = pandaDrive->NonSlip(left,.1,lastLeft);
					rightMotorSpeed = pandaDrive->NonSlip(right, .1,lastRight);
					pandaDrive->SetLeftRightMotorSpeeds(leftMotorSpeed,rightMotorSpeed);
					Wait(.1);
				}
				if (left2 && right2)
				{
					for (int m = 0; m < 25; m++)
					{
						leftMotorSpeed = pandaDrive->NonSlip(left2,.1,lastLeft);
						rightMotorSpeed = pandaDrive->NonSlip(right2, .1,lastRight);
						pandaDrive->SetLeftRightMotorSpeeds(leftMotorSpeed,rightMotorSpeed);
						Wait(.1);
					}
				}
				XAccel();
				YAccel();
			}
		}
	// Do teleop.
	void OperatorControl(void)
	{
		printf("I'm in teleop.\n");
		GetWatchdog().SetEnabled(true);
		// Start a counter so things can happen only periodically.
		unsigned int counter = 0;
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			//if (leftStick->GetRawButton(testButton))
			//	SelfTest();
			if (leftStick->GetRawButton(leftKillButton) && rightStick->GetRawButton(rightKillButton) && rightStick->GetRawButton(rightKillButton2))
			{
				printf("You killed me!\n");
				m_kill = true;
				pandaDrive->SetLeftRightMotorSpeeds(0, 0);
				pandaConv->Drive(0);
				pandaComp->Stop();
				return;
			}
			int retval = pandaDrive->Teleop();
			if (retval & 1) dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Tank drive           ");
			else dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Arcade drive         ");
			if (retval & 2) dslcd->Printf(DriverStationLCD::kUser_Line3, 1, "Nonslip is ON        ");
			else dslcd->Printf(DriverStationLCD::kUser_Line3, 1, "Nonslip is OFF       ");
			dslcd->Printf(DriverStationLCD::kUser_Line4, 1, "                     ");
			dslcd->Printf(DriverStationLCD::kUser_Line4, 1, "Const = %3.1f", pandaDrive->GetAccelConst());
			dslcd->UpdateLCD();
			if (!rightStick->GetRawButton(Trigger))
				pandaDrive->SetLeftRightMotorSpeeds(0, 0);
		    int retval2 = pandaConv->teleop();
		    pandaDump->Teleop();
			if (retval2 & 1) dslcd->Printf(DriverStationLCD::kUser_Line5, 1, "Conv: ON,            ");
			else dslcd->Printf(DriverStationLCD::kUser_Line5, 1, "Conv:OFF,            ");
			if (retval2 & 2) dslcd->Printf(DriverStationLCD::kUser_Line5, 10, " ON");
			else dslcd->Printf(DriverStationLCD::kUser_Line5, 10, "OFF");
			dslcd->UpdateLCD();
		    if (pandaComp->GetPressureSwitchValue() < 60)
		    {
		    	pandaComp->Start();
		    }
		    if (pandaComp->GetPressureSwitchValue() >= 120)
		    {
		    	pandaComp->Stop();
		    }
		    if (!pandaComp->GetPressureSwitchValue()) dslcd->Printf(DriverStationLCD::kUser_Line6, 1, "Compressor: ON       ");
		    else dslcd->Printf(DriverStationLCD::kUser_Line6, 1, "Compressor: OFF      ");
		    dslcd->UpdateLCD();
		    if (counter % 10000 == 0)
		    {
		    	// Do the periodic stuff.
		    	//static float lastX = 0, lastY = 0, startTime = 0;
		    	//if (startTime == 0)
		    	//	startTime = GetTime();
		    	//float x = xAccel->GetAcceleration();
		    	//float y = yAccel->GetAcceleration();
		    	static int last_encoder = 0;
		    	int encoder = rightEncoder->Get();
		    	printf("delta encoder = %d\n", encoder - last_encoder);
		    	last_encoder = encoder;
		    	//if (x != lastX || y != lastY)
		    	//	printf("accel = (%.2f,%.2f) @ %.2f\n", x, y, GetTime() - startTime);
		    	//lastX = x;
		    	//lastY = y;
		    	counter = 0;	
		    }
		    counter++;
		}

	}
};

/*void PandaBot::SelfTest() 
{
	printf("The test code is running.\n");
	pandaDump->TestDump(false, false);
	pandaConv->Drive(0);
	// Test forward
	pandaDrive->SetLeftRightMotorSpeeds(.5, .5);
	printf("Drive forward at half speed for 3 seconds.\n");
	Wait(3);
	pandaDrive->SetLeftRightMotorSpeeds(0, 0);
	Wait (1);
	// Test Backward
	pandaDrive->SetLeftRightMotorSpeeds(-.5, -.5);
	printf("Drive backwards at half speed for 3 seconds.\n");
	Wait(3);
	pandaDrive->SetLeftRightMotorSpeeds(0, 0);
	Wait(1);
	// Test Turn
	pandaDrive->SetLeftRightMotorSpeeds(1, -.5);
	printf("Drive backwards at half speed for 3 seconds.\n");
	Wait(3);
	pandaDrive->SetLeftRightMotorSpeeds(0, 0);
	//Test NonSlip
	float MaxAccel = 1.0;
	for (int i = 0;i<4;i++)
	{
		float motorSpeed = 0;
		float setMotorSpeed = 0;
		pandaDrive->SetAccel(MaxAccel);
		MaxAccel /= 2;
		pandaDrive->SetLeftRightMotorSpeeds(0,0);
		Wait(.5);
		printf("The acceleration constant is %f\n",MaxAccel);
		for (int m = 0;m<20;m++)
		{
			setMotorSpeed = pandaDrive->NonSlip(1,.1,motorSpeed);
			printf("The speed is %f\n",setMotorSpeed);
			pandaDrive->SetLeftRightMotorSpeeds(setMotorSpeed,setMotorSpeed);
			Wait(.1);
		}
	}
	pandaConv->Drive(1);
		printf("Run conveyor belts for 4 seconds.\n");
	Wait(4);
	pandaConv->Drive(0);
	pandaDump->TestDump(true, false);
	Wait(0.3);
	pandaDump->TestDump(false, false);
	Wait(3);
	pandaDump->TestDump(false, true);
	Wait(0.3);
	pandaDump->TestDump(false, false);
}*/

START_ROBOT_CLASS(PandaBot);
